#include "DecoArticulatedBody.h"
#include "GridField.h"
#include "DecoLogger.h"
#include "FluidSparseVector.h"
#include "SimpleFluidCoupler.h"

void IndexJacobianList::AddElement(const Index3& index, const Matd& jacobian)
{
	std::vector<Index3>::iterator it = find(mIndices.begin(), mIndices.end(), index);
	if (it != mIndices.end())
	{
		int pos = it - mIndices.begin();
		mJacobian[pos] = jacobian;
	}
	else
	{
		mIndices.push_back(index);
		mJacobian.push_back(jacobian);
	}
}
bool IndexJacobianList::FindElement(const Index3& index, Matd& jacobian)
{
	std::vector<Index3>::iterator it = find(mIndices.begin(), mIndices.end(), index);
	if (it != mIndices.end())
	{
		int pos = it - mIndices.begin();
		jacobian = mJacobian[pos];
		return true;
	}
	else
	{
		return false;
	}
}

int IndexJacobianList::Size() const
{
  return static_cast<int>(mJacobian.size());
}
void IndexJacobianList::Clear()
{
	mIndices.clear();
	mJacobian.clear();
}

DecoArticulatedObject::DecoArticulatedObject(void) 
:	mModelInfo(NULL),
	mModel(NULL),
	mMesh(NULL),
	mMotionScript(NULL),
	mCurrentTime(0),
	mSimulateType(AST_Cartesian),
	mSimulator(NULL),
	mGeneralizedForce(NULL),
	mMode(TM_Prescribe),
	mCumulativeEnergy(0),
	mCumulativeTorqueAbs(0),
	mCumulativeTorqueSqr(0),
	mCumulativeDeviation(0),
	mCumulativeCollisionVol(0),
	mRotTorque(0)
{
	bIsRenderable = TRUE;
	bIsMovable = TRUE;
	bIsSelectable = TRUE;
	bIsRotatable = FALSE;
	objType = ArticulatedT;
	mLocalToWorld.identity();
	//m_omg = vector3(0, 0, 1);

	mPosFirstCycle = vector3(-999, -999, -999);
	mOriFirstCycle = vector3(-999, -999, -999);
	mDirFirstCycle = vector3(-999, -999, -999);

	for(int i = 0; i < 10; i++)
		mCumulativeIndexedEnergy[i] = 0.0;

}
DecoArticulatedObject::~DecoArticulatedObject(void)
{
	if (mModelInfo)
		delete mModelInfo;
	if (mMesh)
		delete mMesh;
	if (mSimulator)
		delete mSimulator;
	if (mGeneralizedForce)
		delete[] mGeneralizedForce;
}

void DecoArticulatedObject::ReadFromSkeleton(const std::string& path)
{
	name = path;
	ifstream testStrm(path.c_str());
	if(testStrm.fail())
		return;
	testStrm.close();

	char filename[256];
	memset(filename, 0, 256 * sizeof(char));
	strcpy(filename, path.c_str());

	if (mModelInfo)
		delete mModelInfo;

	mModelInfo = new ModelFileInfo;
	

	mModelInfo->loadFile(filename);
	mModel = mModelInfo->getModel();

	if (mMesh)
		delete mMesh;
	mMesh = new DecoSkelMesh();
	mMesh->ConstructFromModel(mModel);

	Vecd q = mModel->getQ();
	coords.SetOrigin(vector3(q[0], q[1], q[2]));
	string scriptPath = path;
	size_t found = scriptPath.find(".skel");
	scriptPath.replace(found, scriptPath.length(), ".script");

	if (mMotionScript)
		delete mMotionScript;
	mMotionScript = new SkeletonScriptReader(mModel, scriptPath);

	int numDofs = mModel->getNumDofs();
	mGeneralizedForce = new FluidSparseVector[numDofs];

}

SkeletonScriptReader* DecoArticulatedObject::GetMotionScript() const
{
	return mMotionScript;
}

void DecoArticulatedObject::SetMotionScript(SkeletonScriptReader* motionScript)
{
	if (mMotionScript)
		delete mMotionScript;
	mMotionScript = motionScript;
}

bool DecoArticulatedObject::SetMotionScriptAmplitude(const std::string& dofName, double value)
{
	if (mMotionScript)
		return mMotionScript->SetAmplitude(dofName, value);
	else
		return false;
}

bool DecoArticulatedObject::SetMotionScriptPeriod(const std::string& dofName, double value)
{
	if (mMotionScript)
		return mMotionScript->SetPeriod(dofName, value);
	else
		return false;

}

bool DecoArticulatedObject::SetMotionScriptPhase(const std::string& dofName, double value)
{
	if (mMotionScript)
		return mMotionScript->SetPhase(dofName, value);
	else
		return false;
}

bool DecoArticulatedObject::SetMotionScriptConstant(const std::string& dofName, double value)
{
	if (mMotionScript)
		return mMotionScript->SetConstant(dofName, value);
	else
		return false;
}

bool DecoArticulatedObject::SetMotionScriptSpline(const std::string& dofName, double period, const std::vector<double>& ctrlPts)
{
	if (mMotionScript)
		return mMotionScript->SetControlPts(dofName, period, ctrlPts);
	else
		return false;
}

bool DecoArticulatedObject::SetMotionScriptSineWarpControlPts(const std::string& name, const std::vector<double>& ctrPts)
{
	if (mMotionScript)
		return mMotionScript->SetSineWarpControlPts(name, ctrPts);
	else
		return false;
}

DecoArticulatedObject* DecoArticulatedObject::CreateFromMesh(const std::string & path)
{
	DecoArticulatedObject* retval = new DecoArticulatedObject();
	retval->ReadFromSkeleton(path);

	vector<char> name(retval->name.begin(), retval->name.end());
	vector<char>::reverse_iterator r1 = find(name.rbegin(), name.rend(), '/');
	vector<char>::reverse_iterator r2 = find(name.rbegin(), name.rend(), '.');
	vector<char> section(r1.base(), r2.base()-1);
	section.push_back('\0');
	DecoConfig::GetSingleton()->GetBool(&(section[0]), "twoway", retval->m_bTwowayCouple);
	DecoConfig::GetSingleton()->GetDouble(&(section[0]), "rho", retval->m_rho);
	INT mode = 0;
	DecoConfig::GetSingleton()->GetBool(&(section[0]), "plan", mode);
	retval->mMode = static_cast<TrackMode>(mode);
	return retval;
}

//#define WATER_OFF

void DecoArticulatedObject::UpdateTransformation()
{
	if (mSimulateType == AST_Cartesian)
	{
		mMesh->SetLocalToWorld(mLocalToWorld);
		(*DecoLogger::GetSingleton()) << mLocalToWorld << "\n";
		mMotionScript->UpdateModel(mCurrentTime);
	}
	else if (mSimulateType == AST_General)
	{
		int numDofs = mModel->getNumDofs();
		double* dofs = new double[numDofs];
		double* vels = new double[numDofs];

		Vecd q = mModel->getQ();
		Vecd qdot = mModel->getQDot();

		for (int i = 0; i < numDofs; ++i)
		{
			dofs[i] = q[i];
			vels[i] = qdot[i];
		}

		mMotionScript->GetCurrentDofsAndVels(mCurrentTime, dofs, vels);
		mSimulator->SetTargetState(dofs, vels);
		delete[] dofs;
		delete[] vels;
	}
}




void DecoArticulatedObject::SetSimulationType(ArticulatedSimType type)
{
	mSimulateType = type;
	if (type == AST_General)
	{
		mSimulator = new ForwardDynamics();
		mSimulator->SetModel(mModel);
		//mModel->getDof(2)->setVelocity(0.01);
	}
}

void DecoArticulatedObject::updateInCartesianCoord(DOUBLE deltaTime)
{
	calculateMass();
	calculateInertia();
	calculateMomentum();
	int numAssociatedCells = static_cast<int>(m_associateGridIndex.size());
	vector3 totalForce(0, 0, 0);
	vector3 totalTorque(0, 0, 0);
	double area = m_grid->stepSize * m_grid->stepSize;
	vector3 force, dirFromOrigin, torque;
	for (int it = 0; it < numAssociatedCells; ++it)
	{
		int i = m_associateGridIndex[it].m_i;
		int j = m_associateGridIndex[it].m_j;
		int k = m_associateGridIndex[it].m_k;
		if (m_grid->isFluidCell(i + 1, j, k))
		{
			force = m_grid->p[i + 1][j][k] * area * vector3(-1, 0, 0);
			vector3 posGlobal = m_grid->GetGridCenter(i + 1, j, k) + vector3(-m_grid->stepSize / 2, 0, 0);
			dirFromOrigin = posGlobal - m_COM;
			torque = CrossProduct(dirFromOrigin, force);

			totalForce += force;	
			totalTorque += torque;
		}
		if (m_grid->isFluidCell(i - 1, j, k))
		{
			force = m_grid->p[i - 1][j][k] * area * vector3(1, 0, 0);

			vector3 posGlobal = m_grid->GetGridCenter(i - 1, j, k) + vector3(m_grid->stepSize / 2, 0, 0);
			dirFromOrigin = posGlobal - m_COM;
			torque = CrossProduct(dirFromOrigin, force);

			totalForce += force;	
			totalTorque += torque;

		}
		if (m_grid->isFluidCell(i, j + 1, k))
		{
			force = m_grid->p[i][j + 1][k] * area * vector3(0, -1, 0);

			vector3 posGlobal = m_grid->GetGridCenter(i, j + 1, k) + vector3(0, -m_grid->stepSize / 2, 0);
			dirFromOrigin = posGlobal - m_COM;
			torque = CrossProduct(dirFromOrigin, force);

			totalForce += force;	
			totalTorque += torque;

		}
		if (m_grid->isFluidCell(i, j - 1, k))
		{
			force = m_grid->p[i][j - 1][k] * area * vector3(0, 1, 0);

			vector3 posGlobal = m_grid->GetGridCenter(i, j - 1, k) + vector3(0, m_grid->stepSize / 2, 0);
			dirFromOrigin = posGlobal - m_COM;
			torque = CrossProduct(dirFromOrigin, force);

			totalForce += force;	
			totalTorque += torque;

		}
		if (m_grid->isFluidCell(i, j, k + 1))
		{
			force = m_grid->p[i][j][k + 1] * area * vector3(0, 0, -1);

			vector3 posGlobal = m_grid->GetGridCenter(i, j, k + 1) + vector3(0, 0, -m_grid->stepSize / 2);
			dirFromOrigin = posGlobal - m_COM;
			torque = CrossProduct(dirFromOrigin, force);

			totalForce += force;	
			totalTorque += torque;

		}
		if (m_grid->isFluidCell(i, j, k - 1))
		{
			force = m_grid->p[i][j][k - 1] * area * vector3(0, 0, 1);

			vector3 posGlobal = m_grid->GetGridCenter(i, j, k - 1) + vector3(0, 0, m_grid->stepSize / 2);
			dirFromOrigin = posGlobal - m_COM;
			torque = CrossProduct(dirFromOrigin, force);

			totalForce += force;	
			totalTorque += torque;

		}
	}
	//totalForce = vector3(0, 0, 0);
	//totalTorque = vector3(0, 0, 0);
	//totalTorque.x = totalTorque.y = totalTorque.z = 0;
	//totalForce.z = totalForce.x = 0;
	updateRigidBody(totalForce, totalTorque, deltaTime);


}

Vecd DecoArticulatedObject::convertForceFromCartesianToGeneral(BodyLink* b, const vector3& posLocal, const vector3& force)
{
	Matd jacobian = mSimulator->CalculateJacobian(b, Vec3d(posLocal.x, posLocal.y, posLocal.z));
	jacobian = trans(jacobian);
	return jacobian * Vec3d(force.x, force.y, force.z);
}



void DecoArticulatedObject::updateInGeneralCoord(DOUBLE deltaTime)
{
	calculateMass();
	int numBodies = mModel->getNumBodyLinks();
	int numDofs = mModel->getNumDofs();
	double area = m_grid->stepSize * m_grid->stepSize;
	vector3 force, torque, dirFromOrigin;
	vector3 totalforce = vector3(0, 0, 0);
	vector3 totaltorque = vector3(0, 0, 0);
	Vecd generalizedForce(numDofs);
	generalizedForce = vl_0;
	for (int ithBody = 0; ithBody < numBodies; ++ithBody)
	{
		int numAssocGrid = static_cast<int>(mGridIndicesForBodies[ithBody].size());
		BodyLink* b = mModel->getBodyLink(ithBody);
		Mat4d transformVL = b->getWorldTransform();
		matrix44 transform = FromMat4dToMatrix44(transformVL);
		matrix44 invTransform = transform.invert();

		for (int it = 0; it < numAssocGrid; ++it)
		{
			int i = mGridIndicesForBodies[ithBody][it].m_i;
			int j = mGridIndicesForBodies[ithBody][it].m_j;
			int k = mGridIndicesForBodies[ithBody][it].m_k;
			if (m_grid->isFluidCell(i + 1, j, k))
			{
				if (ithBody == 5)
					*(DecoLogger::GetSingleton()) << "(" << i + 1 << ", " << j << ", " << k << "): " << m_grid->p[i + 1][j][k] << "\n";
				force = m_grid->p[i + 1][j][k] * area * vector3(-1, 0, 0);
				vector3 posGlobal = m_grid->GetGridCenter(i + 1, j, k) + vector3(-m_grid->stepSize / 2, 0, 0);
				vector3 posLocal = TransformPoint(invTransform, posGlobal);
				Vecd gForce = convertForceFromCartesianToGeneral(b, posLocal, force);
				generalizedForce += gForce;

				dirFromOrigin = posGlobal - m_COM;
				torque = CrossProduct(dirFromOrigin, force);
				totaltorque += torque;
				totalforce += force;

			}
			if (m_grid->isFluidCell(i - 1, j, k))
			{
				if (ithBody == 5)
					*(DecoLogger::GetSingleton()) << "(" << i - 1 << ", " << j << ", " << k << "): " << m_grid->p[i - 1][j][k] << "\n";
				force = m_grid->p[i - 1][j][k] * area * vector3(1, 0, 0);
				vector3 posGlobal = m_grid->GetGridCenter(i - 1, j, k) + vector3(m_grid->stepSize / 2, 0, 0);
				vector3 posLocal = TransformPoint(invTransform, posGlobal);
				Vecd gForce = convertForceFromCartesianToGeneral(b, posLocal, force);
				generalizedForce += gForce;

				dirFromOrigin = posGlobal - m_COM;
				torque = CrossProduct(dirFromOrigin, force);
				totaltorque += torque;
				totalforce += force;


			}
			if (m_grid->isFluidCell(i, j + 1, k))
			{
				if (ithBody == 5)
					*(DecoLogger::GetSingleton()) << "(" << i << ", " << j + 1 << ", " << k << "): " << m_grid->p[i][j + 1][k] << "\n";
				force = m_grid->p[i][j + 1][k] * area * vector3(0, -1, 0);
				vector3 posGlobal = m_grid->GetGridCenter(i, j + 1, k) + vector3(0, -m_grid->stepSize / 2, 0);
				vector3 posLocal = TransformPoint(invTransform, posGlobal);
				Vecd gForce = convertForceFromCartesianToGeneral(b, posLocal, force);
				generalizedForce += gForce;

				dirFromOrigin = posGlobal - m_COM;
				torque = CrossProduct(dirFromOrigin, force);
				totaltorque += torque;
				totalforce += force;

			}
			if (m_grid->isFluidCell(i, j - 1, k))
			{
				if (ithBody == 5)
					*(DecoLogger::GetSingleton()) << "(" << i << ", " << j - 1 << ", " << k << "): " << m_grid->p[i][j - 1][k] << "\n";
				force = m_grid->p[i][j - 1][k] * area * vector3(0, 1, 0);
				vector3 posGlobal = m_grid->GetGridCenter(i, j - 1, k) + vector3(0, m_grid->stepSize / 2, 0);
				vector3 posLocal = TransformPoint(invTransform, posGlobal);
				Vecd gForce = convertForceFromCartesianToGeneral(b, posLocal, force);
				generalizedForce += gForce;

				dirFromOrigin = posGlobal - m_COM;
				torque = CrossProduct(dirFromOrigin, force);
				totaltorque += torque;
				totalforce += force;


			}
			if (m_grid->isFluidCell(i, j, k + 1))
			{
				if (ithBody == 5)
					*(DecoLogger::GetSingleton()) << "(" << i << ", " << j << ", " << k + 1 << "): " << m_grid->p[i][j][k + 1] << "\n";
				force = m_grid->p[i][j][k + 1] * area * vector3(0, 0, -1);
				vector3 posGlobal = m_grid->GetGridCenter(i, j, k + 1) + vector3(0, 0, -m_grid->stepSize / 2);
				vector3 posLocal = TransformPoint(invTransform, posGlobal);
				Vecd gForce = convertForceFromCartesianToGeneral(b, posLocal, force);
				generalizedForce += gForce;

				dirFromOrigin = posGlobal - m_COM;
				torque = CrossProduct(dirFromOrigin, force);
				totaltorque += torque;
				totalforce += force;

			}
			if (m_grid->isFluidCell(i, j, k - 1))
			{
				if (ithBody == 5)
					*(DecoLogger::GetSingleton()) << "(" << i << ", " << j << ", " << k - 1 << "): " << m_grid->p[i][j][k - 1] << "\n";
				force = m_grid->p[i][j][k - 1] * area * vector3(0, 0, 1);
				vector3 posGlobal = m_grid->GetGridCenter(i, j, k - 1) + vector3(0, 0, m_grid->stepSize / 2);
				vector3 posLocal = TransformPoint(invTransform, posGlobal);
				Vecd gForce = convertForceFromCartesianToGeneral(b, posLocal, force);
				generalizedForce += gForce;

				dirFromOrigin = posGlobal - m_COM;
				torque = CrossProduct(dirFromOrigin, force);
				totaltorque += torque;
				totalforce += force;

			}
		}
	}
	const SceneBuilderInfo& info = DecoConfig::GetSingleton()->GetSceneBuilderInfo();
	if (info.mSimulationDim == 2)
	{
		generalizedForce[2] = 0; //hack balance controller
		for (int i = 3; i < numDofs; ++i)
		{
			if ((i + 1) % 3)
				generalizedForce[i] = 0;
		}
	}
	
	Matd massMat(numDofs, numDofs, vl_0);
	mSimulator->CalculateMMatrix(massMat);
	Matd invMassMat = inv(massMat);
	Vecd qDotDotExternal = invMassMat * generalizedForce;
	qDotDotExternal *= deltaTime;
	for (int i = 0; i < numDofs; ++i)
	{
		double originalVel = mModel->getDof(i)->getVelocity();
		mModel->getDof(i)->setVelocity(originalVel + qDotDotExternal[i]);
	}

	mRotTorque = qDotDotExternal[4];

#ifdef WATER_OFF
	for (int i = 0; i < 6; ++i)
	{
		mModel->getDof(i)->setVelocity(0);
	}
#endif

	//(*DecoLogger::GetSingleton()) << "External forces are: ";
	//for (int i = 0; i < 22; ++i)
	//{
	//	(*DecoLogger::GetSingleton()) << generalizedForce[i] << " ";
	//}
	//(*DecoLogger::GetSingleton()) << "\n";

	//Vecd internalForce = mSimulator->GetInternalForce();
	//(*DecoLogger::GetSingleton()) << "Internal forces are: ";
	//for (int i = 0; i < 12; ++i)
	//{
	//	(*DecoLogger::GetSingleton()) << internalForce[i] << " ";
	//}
	//(*DecoLogger::GetSingleton()) << "\n";

	//double q[22];
	//(*DecoLogger::GetSingleton()) << "Current DOFs are: ";
	//for (int i = 0; i < 22; ++i)
	//{
	//	q[i] = mModel->getDof(i)->getValue();
	//	(*DecoLogger::GetSingleton()) << q[i] << " ";
	//}
	//(*DecoLogger::GetSingleton()) << "\n";

	//double qdot[22];
	//(*DecoLogger::GetSingleton()) << "Current qdots are: ";
	//for (int i = 0; i < 22; ++i)
	//{
	//	qdot[i] = mModel->getDof(i)->getVelocity();
	//	(*DecoLogger::GetSingleton()) << qdot[i] << " ";
	//}
	//(*DecoLogger::GetSingleton()) << "\n";

}

void DecoArticulatedObject::Update(DOUBLE deltaTime)
{
	if (m_bTwowayCouple)
	{
		if (mSimulateType == AST_Cartesian)
			updateInCartesianCoord(deltaTime);
		else if (mSimulateType == AST_General)
			updateInGeneralCoord(deltaTime);
	}
	mCurrentTime += deltaTime;
}

void DecoArticulatedObject::Reset()
{
	if (mModelInfo)
		delete mModelInfo;

	mModelInfo = new ModelFileInfo;

	char filename[256];
	memset(filename, 0, 256 * sizeof(char));
	strcpy(filename, name.c_str());

	mModelInfo->loadFile(filename);
	mModel = mModelInfo->getModel();

	int numLinks = mModel->getNumBodyLinks();
	std::vector<BodyLink*> links;
	for (int i = 0; i < numLinks; ++i)
		links.push_back(mModel->getBodyLink(i));
	mMesh->SetBodyLinks(links);

	Vecd q = mModel->getQ();
	coords.SetOrigin(vector3(q[0], q[1], q[2]));

	mCurrentTime = 0;
	mCumulativeEnergy = 0;
	mCumulativeTorqueAbs = 0;
	mCumulativeTorqueSqr = 0;
	mCumulativeDeviation = 0;
	mCumulativeCollisionVol = 0;
}

void DecoArticulatedObject::updateRigidBody(const vector3& force, const vector3& torque, DOUBLE deltaTime)
{
	mMesh->SetCOM(m_COM);

	vector3 forceWithGravity = force;// + m_mass * vector3(0, -9.8, 0);
	m_linMomentum += deltaTime * forceWithGravity;
	m_vel = m_linMomentum / m_mass;

	vector3 offset = m_vel * deltaTime;
	
	m_angMomentum += deltaTime * torque;
	m_omg = m_inertia.invert() * m_angMomentum;

	double angle = m_omg.length() * deltaTime;
	vector3 rotDir = m_omg;
	rotDir.normalize();

	Quaternion qdot(angle, rotDir);

	matrix44 offsetMat = TranslateMatrix44(offset.x, offset.y, offset.z);
	vector3 offsetFromCOM = vector3(-m_COM.x, -m_COM.y, -m_COM.z);

	matrix44 offsetFromCOMMat = TranslateMatrix44(offsetFromCOM.x, offsetFromCOM.y, offsetFromCOM.z);
	matrix44 offsetToCOMMat = TranslateMatrix44(-offsetFromCOM.x, -offsetFromCOM.y, -offsetFromCOM.z);
	matrix44 rotMat;
	qdot.Matrix(rotMat);
	matrix44 currentLocalToWorld = mMesh->GetLocalToWorld();
	mLocalToWorld = offsetMat *offsetToCOMMat * rotMat * offsetFromCOMMat * currentLocalToWorld;

	//UpdateTransformation();
	(*DecoLogger::GetSingleton()) << "Force  :" << forceWithGravity << "\n";
	(*DecoLogger::GetSingleton()) << "Torque :" << torque << "\n";
}

void DecoArticulatedObject::GetQDot(std::vector<double>& qdot)
{
	qdot.clear();
	int numDofs = mModel->getNumDofs();
	Vecd qdotVec = mModel->getQDot();
	for (int i = 0; i < numDofs; ++i)
	{
		qdot.push_back(qdotVec[i]);
	}
}

void DecoArticulatedObject::GetDesiredQandQDot(double time, std::vector<double>& q, std::vector<double>& qdot)
{
	int numDofs = mModel->getNumDofs();
	q.clear();
	qdot.clear();
	double* dofs = new double[numDofs];
	double* vels = new double[numDofs];
	if (mMotionScript)
		mMotionScript->GetCurrentDofsAndVels(time, dofs, vels);
	for (int i = 0; i < numDofs; ++i)
	{
		if (mMotionScript)
		{
			q.push_back(dofs[i]);
			qdot.push_back(vels[i]);	

		}
		else
		{
			q.push_back(0);
			qdot.push_back(0);
		}

	}
	delete[] dofs;
	delete[] vels;

}

void DecoArticulatedObject::GetQ(std::vector<double>& q)
{
	q.clear();
	int numDofs = mModel->getNumDofs();
	Vecd qVec = mModel->getQ();
	for (int i = 0; i < numDofs; ++i)
	{
		q.push_back(qVec[i]);
	}
}

void DecoArticulatedObject::SetQDot(const std::vector<double>& qdot, bool bSetFirstSixDofs)
{
	int nDofs = static_cast<int>(qdot.size());
	assert(nDofs == mModel->getNumDofs());

	int startIdx = 0;
	if (!bSetFirstSixDofs)
		startIdx = 6;
	

	for (int i = startIdx; i < nDofs; ++i)
	{
		mModel->getDof(i)->setVelocity(qdot[i]);
	}
}

void DecoArticulatedObject::UpdateBeforeCoupling(DOUBLE deltaTime, BOOL bSimplifiedFluid)
{
	if (mSimulateType == AST_General)
	{
		int numDofs = mModel->getNumDofs();
		Vecd externalForce(numDofs, vl_0);
		if (bSimplifiedFluid)
		{
			UpdateTransformation();

			SimpleFluidCoupler simpleFluid(this);
			externalForce = simpleFluid.GetExternalTorque();
		}
		mSimulator->SetExternalForce(externalForce);
		CumulateDeviationFromCircle(vector3(0, 0.5, 0), 0.5, -0.3, mCurrentTime);
		vector3 energy = mSimulator->Update(deltaTime, mCumulativeIndexedEnergy, mMode);
		CumulateEnergyConsumption(energy.x, energy.y, energy.z);
		if (bSimplifiedFluid)
		{
			mCurrentTime += deltaTime;
		}
	}
}

void DecoArticulatedObject::UpdateToTarget()
{
	mSimulator->SetStateAndVelAsTarget();
}

vector3 DecoArticulatedObject::SampleVelocity(const vector3& pos)
{
	int numBodyLinks = mModel->getNumBodyLinks();
	bool bProcessed = false;
	Vecd vel = vl_0;
	vector3 worldVel;
	UINT i, j, k;

	vector3 testCellPos = m_grid->fromWorldToLocalGrid(pos, i, j, k);
	;
	
	int ithBody = -1;
	for (int l = 0; l < mGridIndicesForBodies.size(); ++l)
	{
		IndiceListT::iterator it = find(mGridIndicesForBodies[l].begin(), mGridIndicesForBodies[l].end(), Index3(i, j, k));
		IndiceListT::iterator itIMinus = find(mGridIndicesForBodies[l].begin(), mGridIndicesForBodies[l].end(), Index3(i - 1, j, k));
		IndiceListT::iterator itJMinus = find(mGridIndicesForBodies[l].begin(), mGridIndicesForBodies[l].end(), Index3(i, j - 1, k));
		IndiceListT::iterator itKMinus = find(mGridIndicesForBodies[l].begin(), mGridIndicesForBodies[l].end(), Index3(i, j, k - 1));
		IndiceListT::iterator itIPlus = find(mGridIndicesForBodies[l].begin(), mGridIndicesForBodies[l].end(), Index3(i + 1, j, k));
		IndiceListT::iterator itJPlus = find(mGridIndicesForBodies[l].begin(), mGridIndicesForBodies[l].end(), Index3(i, j + 1, k));
		IndiceListT::iterator itKPlus = find(mGridIndicesForBodies[l].begin(), mGridIndicesForBodies[l].end(), Index3(i, j, k + 1));

		if (it != mGridIndicesForBodies[l].end() || itIMinus != mGridIndicesForBodies[l].end() || itJMinus != mGridIndicesForBodies[l].end() || itKMinus != mGridIndicesForBodies[l].end()
			|| itIPlus != mGridIndicesForBodies[l].end() || itJPlus != mGridIndicesForBodies[l].end() || itKPlus != mGridIndicesForBodies[l].end())
		{
			ithBody = l;
			break;
		}
	}
	//assert(ithBody != -1);
	if (ithBody == -1)
	{
		(*DecoLogger::GetSingleton()) << "Warning: cannot find corresponding velocity at position " << pos << "\n";
		return vector3(0, 0, 0);
	}
	BodyLink* b = mModel->getBodyLink(ithBody);
	Vec3d posVL(pos.x, pos.y, pos.z);

	Mat4d localToWorld = b->getWorldTransform();
	matrix44 localToWorld44 = FromMat4dToMatrix44(localToWorld);

	matrix44 worldToLocal44 = (mLocalToWorld * localToWorld44).invert();
	vector4 homoLocalPos = worldToLocal44 * vector4(pos.x, pos.y, pos.z, 1.0);
	vector3 localPos(homoLocalPos.x, homoLocalPos.y, homoLocalPos.z);

	Matd Jacobian = calculateJacobian(b, localPos);
	Vecd qdot = mModel->getQDot();
	vel = Jacobian * qdot;
	worldVel = vector3(vel[0], vel[1], vel[2]);
	//worldVel = mLocalToWorld * worldVel;
	worldVel += m_vel;
	vector3 velocityFromRotation = CrossProduct(m_omg, pos - m_COM);
	worldVel += velocityFromRotation;

	return worldVel;
}

Matd DecoArticulatedObject::calculateJacobian(BodyLink* b, const vector3& localPos)
{
	int numDofs = mModel->getNumDofs();
	Matd mat(3, numDofs);
	mat.MakeZero();

	int numDof = static_cast<int>(mModel->getNumDofs());

	for (int i = 0; i < numDof; ++i)
	{
		Dof* q = mModel->getDof(i);
		Vec4d homoLocalPos(localPos.x, localPos.y, localPos.z, 1);
		Vec4d cDeriv = b->getFirstDerivative(&homoLocalPos, q);

		mat[0][i] = cDeriv[0];
		mat[1][i] = cDeriv[1];
		mat[2][i] = cDeriv[2];
	}
	return mat;
}

void DecoArticulatedObject::PrepareImplicitTwowayCouplingMatrix(int dim)
{
	if (mSimulateType == AST_Cartesian)
	{
		prepareCouplingMatrixForRigidBody(dim);
	}
	else if (mSimulateType == AST_General)
	{

		if (m_bTwowayCouple)
		{
			int numBodies = mModel->getNumBodyLinks();
			int numDofs = mModel->getNumDofs();

			for (int ithDof = 0; ithDof < numDofs; ++ithDof)
			{
				mGeneralizedForce[ithDof].SetDimension(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement, dim);
				mGeneralizedForce[ithDof].SetZero();
			}
			mUJacobianList.Clear();
			mVJacobianList.Clear();
			mWJacobianList.Clear();

			Matd massMat(numDofs, numDofs, vl_0);
			mSimulator->CalculateMMatrix(massMat);
			Matd invMassMat = inv(massMat);

			double area = m_grid->stepSize * m_grid->stepSize;
			vector3 force;
			for (int ithBody = 0; ithBody < numBodies; ++ithBody)
			{
				int numAssocGrid = static_cast<int>(mGridIndicesForBodies[ithBody].size());
				BodyLink* b = mModel->getBodyLink(ithBody);
				Mat4d transformVL = b->getWorldTransform();
				matrix44 transform = FromMat4dToMatrix44(transformVL);
				matrix44 invTransform = transform.invert();

				for (int it = 0; it < numAssocGrid; ++it)
				{
					int i = mGridIndicesForBodies[ithBody][it].m_i;
					int j = mGridIndicesForBodies[ithBody][it].m_j;
					int k = mGridIndicesForBodies[ithBody][it].m_k;

					if (m_grid->isFluidCell(i + 1, j, k))
					{
						force = area * vector3(-1, 0, 0);
						vector3 posGlobal = m_grid->GetGridCenter(i + 1, j, k) + vector3(-m_grid->stepSize / 2, 0, 0);
						vector3 posLocal = TransformPoint(invTransform, posGlobal);
						Matd jacobian = mSimulator->CalculateJacobian(b, Vec3d(posLocal.x, posLocal.y, posLocal.z));
						mUJacobianList.AddElement(Index3(i + 1, j, k), jacobian);
						jacobian = trans(jacobian);
						Matd invMassMultJacob = invMassMat * jacobian;
						Vecd generalizedForce = invMassMultJacob * Vec3d(force.x, force.y, force.z);

						for (int ithDof = 0; ithDof < numDofs; ++ithDof)
						{
							double originalValue = mGeneralizedForce[ithDof].GetValue(i + 1, j, k);
							mGeneralizedForce[ithDof].SetValue(i + 1, j, k, originalValue + generalizedForce[ithDof]);
						}
						

					}
					if (m_grid->isFluidCell(i - 1, j, k))
					{
						force = area * vector3(1, 0, 0);
						vector3 posGlobal = m_grid->GetGridCenter(i - 1, j, k) + vector3(m_grid->stepSize / 2, 0, 0);
						vector3 posLocal = TransformPoint(invTransform, posGlobal);
						Matd jacobian = mSimulator->CalculateJacobian(b, Vec3d(posLocal.x, posLocal.y, posLocal.z));
						mUJacobianList.AddElement(Index3(i, j, k), jacobian);
						jacobian = trans(jacobian);
						Matd invMassMultJacob = invMassMat * jacobian;
						Vecd generalizedForce = invMassMultJacob * Vec3d(force.x, force.y, force.z);

						for (int ithDof = 0; ithDof < numDofs; ++ithDof)
						{
							double originalValue = mGeneralizedForce[ithDof].GetValue(i - 1, j, k);
							mGeneralizedForce[ithDof].SetValue(i - 1, j, k, originalValue + generalizedForce[ithDof]);
						}

					}
					if (m_grid->isFluidCell(i, j + 1, k))
					{
						force = area * vector3(0, -1, 0);
						vector3 posGlobal = m_grid->GetGridCenter(i, j + 1, k) + vector3(0, -m_grid->stepSize / 2, 0);
						vector3 posLocal = TransformPoint(invTransform, posGlobal);
						Matd jacobian = mSimulator->CalculateJacobian(b, Vec3d(posLocal.x, posLocal.y, posLocal.z));
						mVJacobianList.AddElement(Index3(i, j + 1, k), jacobian);
						jacobian = trans(jacobian);
						Matd invMassMultJacob = invMassMat * jacobian;
						Vecd generalizedForce = invMassMultJacob * Vec3d(force.x, force.y, force.z);

						for (int ithDof = 0; ithDof < numDofs; ++ithDof)
						{
							double originalValue = mGeneralizedForce[ithDof].GetValue(i, j + 1, k);
							mGeneralizedForce[ithDof].SetValue(i, j + 1, k, originalValue + generalizedForce[ithDof]);
						}
					}
					if (m_grid->isFluidCell(i, j - 1, k))
					{
						force = area * vector3(0, 1, 0);
						vector3 posGlobal = m_grid->GetGridCenter(i, j - 1, k) + vector3(0, m_grid->stepSize / 2, 0);
						vector3 posLocal = TransformPoint(invTransform, posGlobal);
						Matd jacobian = mSimulator->CalculateJacobian(b, Vec3d(posLocal.x, posLocal.y, posLocal.z));
						mVJacobianList.AddElement(Index3(i, j, k), jacobian);
						jacobian = trans(jacobian);
						Matd invMassMultJacob = invMassMat * jacobian;
						Vecd generalizedForce = invMassMultJacob * Vec3d(force.x, force.y, force.z);

						for (int ithDof = 0; ithDof < numDofs; ++ithDof)
						{
							double originalValue = mGeneralizedForce[ithDof].GetValue(i, j - 1, k);
							mGeneralizedForce[ithDof].SetValue(i, j - 1, k, originalValue + generalizedForce[ithDof]);
						}

					}
					if (m_grid->isFluidCell(i, j, k + 1))
					{
						force = area * vector3(0, 0, -1);
						vector3 posGlobal = m_grid->GetGridCenter(i, j, k + 1) + vector3(0, 0, -m_grid->stepSize / 2);
						vector3 posLocal = TransformPoint(invTransform, posGlobal);
						Matd jacobian = mSimulator->CalculateJacobian(b, Vec3d(posLocal.x, posLocal.y, posLocal.z));
						mWJacobianList.AddElement(Index3(i, j, k + 1), jacobian);
						jacobian = trans(jacobian);
						Matd invMassMultJacob = invMassMat * jacobian;
						Vecd generalizedForce = invMassMultJacob * Vec3d(force.x, force.y, force.z);

						for (int ithDof = 0; ithDof < numDofs; ++ithDof)
						{
							double originalValue = mGeneralizedForce[ithDof].GetValue(i, j, k + 1);
							mGeneralizedForce[ithDof].SetValue(i, j, k + 1, originalValue + generalizedForce[ithDof]);
						}
					}
					if (m_grid->isFluidCell(i, j, k - 1))
					{
						force = area * vector3(0, 0, 1);
						vector3 posGlobal = m_grid->GetGridCenter(i, j, k - 1) + vector3(0, 0, m_grid->stepSize / 2);
						vector3 posLocal = TransformPoint(invTransform, posGlobal);
						Matd jacobian = mSimulator->CalculateJacobian(b, Vec3d(posLocal.x, posLocal.y, posLocal.z));
						mWJacobianList.AddElement(Index3(i, j, k), jacobian);
						jacobian = trans(jacobian);
						Matd invMassMultJacob = invMassMat * jacobian;
						Vecd generalizedForce = invMassMultJacob * Vec3d(force.x, force.y, force.z);
						for (int ithDof = 0; ithDof < numDofs; ++ithDof)
						{
							double originalValue = mGeneralizedForce[ithDof].GetValue(i, j, k - 1);
							mGeneralizedForce[ithDof].SetValue(i, j, k - 1, originalValue + generalizedForce[ithDof]);
						}

					}
				}
			}

		}
	}
}

DecoRenderData* DecoArticulatedObject::GetRenderData(int ithBody)
{
	return mMesh->GetRenderDataFromIthBody(ithBody);
}

DecoRenderData* DecoArticulatedObject::GetRenderData()
{
	return mMesh;
};

Model* DecoArticulatedObject::GetModel() const
{
	return mModel;
}

int DecoArticulatedObject::GetNumBodies() const
{
	return mModel->getNumBodyLinks();
}

int DecoArticulatedObject::GetNumDofs() const
{
	return mModel->getNumDofs();
}

FluidSparseVector DecoArticulatedObject::GetPressureContributionToUFaceAcc(int i, int j, int k, int dim)
{
	if (mSimulateType == AST_Cartesian)
		return getPressureContributionToUFaceAccForRigidBody(i, j, k, dim);
	else if (mSimulateType == AST_General)
	{
		int numDofs = mModel->getNumDofs();
		FluidSparseVector ret(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement, dim);
		if (m_grid->isFluidCell(i - 1, j, k) && m_grid->isSolidCell(i, j, k))
		{
			Matd jacobian(3, numDofs, vl_0);
			bool bFound = mUJacobianList.FindElement(Index3(i, j, k), jacobian);
			if (!bFound)
			{
				(*DecoLogger::GetSingleton()) << "UJacobian not found at face (" << i << ", " << j << ", " << k <<").\n";
				*((int*)0) = 0;
			}
			for (int ithBody = 0; ithBody < numDofs; ++ithBody)
			{
				ret += (1.0 - m_grid->uFluidVol[i][j][k]) * jacobian[0][ithBody] * mGeneralizedForce[ithBody];
			}
		}
		else if (m_grid->isFluidCell(i, j, k) && m_grid->isSolidCell(i - 1, j, k))
		{
			Matd jacobian(3, numDofs, vl_0);
			bool bFound = mUJacobianList.FindElement(Index3(i, j, k), jacobian);
			if (!bFound)
			{
				(*DecoLogger::GetSingleton()) << "UJacobian not found at face (" << i << ", " << j << ", " << k <<").\n";
				*((int*)0) = 0;			
			}
			for (int ithBody = 0; ithBody < numDofs; ++ithBody)
			{
				ret -= (1.0 - m_grid->uFluidVol[i][j][k]) * jacobian[0][ithBody] * mGeneralizedForce[ithBody];
			}
		}
		else
		{
			(*DecoLogger::GetSingleton()) << "Should not reach here! GetPressureContributionToUFaceAcc(" << i << ", " << j << ", " << k <<").\n";
		}
		return ret;
	}
	else
	{
		FluidSparseVector ret(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement, dim);
		return ret;
	}
}
FluidSparseVector DecoArticulatedObject::GetPressureContributionToVFaceAcc(int i, int j, int k, int dim)
{
	if (mSimulateType == AST_Cartesian)
		return getPressureContributionToVFaceAccForRigidBody(i, j, k, dim);
	else if (mSimulateType == AST_General)
	{
		int numDofs = mModel->getNumDofs();
		FluidSparseVector ret(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement, dim);
		if (m_grid->isFluidCell(i, j - 1, k) && m_grid->isSolidCell(i, j, k))
		{
			Matd jacobian(3, numDofs, vl_0);
			bool bFound = mVJacobianList.FindElement(Index3(i, j, k), jacobian);
			if (!bFound)
			{
				(*DecoLogger::GetSingleton()) << "VJacobian not found at face (" << i << ", " << j << ", " << k <<").\n";
				*((int*)0) = 0;
			}
			for (int ithBody = 0; ithBody < numDofs; ++ithBody)
			{
				ret += (1.0 - m_grid->vFluidVol[i][j][k]) * jacobian[1][ithBody] * mGeneralizedForce[ithBody];
			}
		}
		else if (m_grid->isFluidCell(i, j, k) && m_grid->isSolidCell(i, j - 1, k))
		{
			Matd jacobian(3, numDofs, vl_0);
			bool bFound = mVJacobianList.FindElement(Index3(i, j, k), jacobian);
			if (!bFound)
			{
				(*DecoLogger::GetSingleton()) << "VJacobian not found at face (" << i << ", " << j << ", " << k <<").\n";
				*((int*)0) = 0;			
			}
			for (int ithBody = 0; ithBody < numDofs; ++ithBody)
			{
				ret -= (1.0 - m_grid->vFluidVol[i][j][k]) * jacobian[1][ithBody] * mGeneralizedForce[ithBody];
			}
		}
		else
		{
			(*DecoLogger::GetSingleton()) << "Should not reach here! GetPressureContributionToVFaceAcc(" << i << ", " << j << ", " << k <<").\n";
		}
		return ret;
	}
	else
	{
		FluidSparseVector ret(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement, dim);
		return ret;
	}
}

FluidSparseVector DecoArticulatedObject::GetPressureContributionToWFaceAcc(int i, int j, int k, int dim)
{
	if (mSimulateType == AST_Cartesian)
		return getPressureContributionToWFaceAccForRigidBody(i, j, k, dim);
	else if (mSimulateType == AST_General)
	{
		int numDofs = mModel->getNumDofs();
		FluidSparseVector ret(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement, dim);
		if (m_grid->isFluidCell(i, j, k - 1) && m_grid->isSolidCell(i, j, k))
		{
			Matd jacobian(3, numDofs, vl_0);
			bool bFound = mWJacobianList.FindElement(Index3(i, j, k), jacobian);
			if (!bFound)
			{
				(*DecoLogger::GetSingleton()) << "WJacobian not found at face (" << i << ", " << j << ", " << k <<").\n";
				*((int*)0) = 0;
			}
			for (int ithBody = 0; ithBody < numDofs; ++ithBody)
			{
				ret += (1.0 - m_grid->wFluidVol[i][j][k]) * jacobian[2][ithBody] * mGeneralizedForce[ithBody];
			}
		}
		else if (m_grid->isFluidCell(i, j, k) && m_grid->isSolidCell(i, j, k - 1))
		{
			Matd jacobian(3, numDofs, vl_0);
			bool bFound = mWJacobianList.FindElement(Index3(i, j, k), jacobian);
			if (!bFound)
			{
				(*DecoLogger::GetSingleton()) << "WJacobian not found at face (" << i << ", " << j << ", " << k <<").\n";
				*((int*)0) = 0;			
			}
			for (int ithBody = 0; ithBody < numDofs; ++ithBody)
			{
				ret -= (1.0 - m_grid->wFluidVol[i][j][k]) * jacobian[2][ithBody] * mGeneralizedForce[ithBody];
			}
		}
		else
		{
			(*DecoLogger::GetSingleton()) << "Should not reach here! GetPressureContributionToWFaceAcc(" << i << ", " << j << ", " << k <<").\n";
		}
		return ret;
	}
	else
	{
		FluidSparseVector ret(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement, dim);
		return ret;
	}
}

bool DecoArticulatedObject::GetJacobianAtUFace(int i, int j, int k, Matd& jacobian)
{
	return mUJacobianList.FindElement(Index3(i, j, k), jacobian);
}

bool DecoArticulatedObject::GetJacobianAtVFace(int i, int j, int k, Matd& jacobian)
{
	return mVJacobianList.FindElement(Index3(i, j, k), jacobian);

}

bool DecoArticulatedObject::GetJacobianAtWFace(int i, int j, int k, Matd& jacobian)
{
	return mWJacobianList.FindElement(Index3(i, j, k), jacobian);

}

double DecoArticulatedObject::GetMass()
{
	return mModel->getMass();
}

vector3 DecoArticulatedObject::GetCumulativeEnergyConsumption()
{
	return vector3(mCumulativeEnergy, mCumulativeTorqueAbs, mCumulativeTorqueSqr);
}

void DecoArticulatedObject::CumulateEnergyConsumption(double energy, double torqueAbs, double torqueSqr)
{
  //	assert(energy >= 0);
  if (energy < 0)
    printf("Weird energy conumption: %f\n", energy);
  mCumulativeEnergy += energy;
  mCumulativeTorqueAbs += torqueAbs;
  mCumulativeTorqueSqr += torqueSqr;
}

void DecoArticulatedObject::CumulateCollisionVolume(double vol)
{
	mCumulativeCollisionVol += vol;
}

double DecoArticulatedObject::GetCumulativeCollisionVolume()
{
	return mCumulativeCollisionVol;
}

void DecoArticulatedObject::CumulateDeviationFromCircle(const vector3& center, double radius, double omega, double currentTime)
{
	double startAngle = 0;
	if (center.y > 0)
	{
		startAngle = -PI / 2;
	}
	else
	{
		startAngle = PI / 2;
	}
	double currentAngleToBe = startAngle + omega * currentTime;
	std::vector<double> q;
	GetQ(q);
	double currentPosXToBe = radius * cos(currentAngleToBe) + center.x;
	double currentPosYToBe = radius * sin(currentAngleToBe) + center.y;
	double currentPosZToBe = 0;
	double currentOrientationZToBe = 0;
	if (omega >= 0)
		currentOrientationZToBe = currentAngleToBe - PI / 2;
	else
		currentOrientationZToBe = currentAngleToBe + PI / 2;
	double deviationIncrement = powf(currentPosXToBe - q[0], 2) + powf(currentPosYToBe - q[1], 2) + powf(currentPosZToBe - q[2], 2) + powf(currentOrientationZToBe - q[5], 2);
	(*DecoLogger::GetSingleton()) << "q: " << q[0] << " " << q[1] << " " << q[2] << " "  << q[3] << " " << q[4] << " " << q[5] << " \n";
	(*DecoLogger::GetSingleton()) << "currentTime: " << currentTime << "\n";
	(*DecoLogger::GetSingleton()) << "tobe: " << currentPosXToBe << " " << currentPosYToBe << " " << currentPosZToBe << " " << currentOrientationZToBe << "\n";
	mCumulativeDeviation += deviationIncrement;

}

double DecoArticulatedObject::GetCumulativeDeviation()
{
	return mCumulativeDeviation;
}

void DecoArticulatedObject::ApplyMotion(const SearcherInfo& info, double* params, int numArray)
{
	int numBounds = static_cast<int>(info.mJointBounds.size());
	if (numArray != numBounds)
	{
		printf("The number of parameters %d and the number of joint bounds %d do not match\n", numArray, numBounds);
		(*DecoLogger::GetSingleton()) << "The number of parameters " << numArray << " and the number of joint bounds " << numBounds << " do not match!\n";
		return;
	}
	for (int i = 0; i < numBounds; ++i)
	{
		if (info.mJointBounds[i].mType == JBT_Amplitude)
		{
			mMotionScript->SetAmplitude(info.mJointBounds[i].mJointName, params[i]);
		}
		else if (info.mJointBounds[i].mType == JBT_Phase)
		{
			mMotionScript->SetPhase(info.mJointBounds[i].mJointName, params[i]);
		}
		else if (info.mJointBounds[i].mType == JBT_Constant)
		{
			mMotionScript->SetConstant(info.mJointBounds[i].mJointName, params[i]);
		}
		else if (info.mJointBounds[i].mType == JBT_Min)
		{
			mMotionScript->SetRangeMin(info.mJointBounds[i].mJointName, params[i]);
		}
		else if (info.mJointBounds[i].mType == JBT_Max)
		{
			mMotionScript->SetRangeMax(info.mJointBounds[i].mJointName, params[i]);
		}
		else if (info.mJointBounds[i].mType == JBT_StraightLen1)
		{
			mMotionScript->SetStaright1(info.mJointBounds[i].mJointName, params[i]);
		}
		else if (info.mJointBounds[i].mType == JBT_StraightLen2)
		{
			mMotionScript->SetStaright2(info.mJointBounds[i].mJointName, params[i]);
		}
		else if (info.mJointBounds[i].mType == JBT_HalfRatio)
		{
			mMotionScript->SetHalfRatio(info.mJointBounds[i].mJointName, params[i]);
		}
		else if (info.mJointBounds[i].mType == JBT_Warp0)
		{
			mMotionScript->SetSineWarpControlPts(info.mJointBounds[i].mJointName, 0, params[i]);
		}
		else if (info.mJointBounds[i].mType == JBT_Warp1)
		{
			mMotionScript->SetSineWarpControlPts(info.mJointBounds[i].mJointName, 1, params[i]);
		}
		else if (info.mJointBounds[i].mType == JBT_Warp2)
		{
			mMotionScript->SetSineWarpControlPts(info.mJointBounds[i].mJointName, 2, params[i]);
		}
		else if (info.mJointBounds[i].mType == JBT_Warp3)
		{
			mMotionScript->SetSineWarpControlPts(info.mJointBounds[i].mJointName, 3, params[i]);
		}

	}
	int numConstraints = static_cast<int>(info.mJointConstraints.size());
	for (int i = 0; i < numConstraints; ++i)
	{
		if (info.mJointConstraints[i].mBoundType == JBT_Amplitude)
		{
			switch (info.mJointConstraints[i].mType)
			{
			case JSCT_Equal:
				mMotionScript->SetAmplitude(info.mJointConstraints[i].mJointName1, params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			case JSCT_Negate:
				mMotionScript->SetAmplitude(info.mJointConstraints[i].mJointName1, -params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			}
		}
		else if (info.mJointConstraints[i].mBoundType == JBT_Phase)
		{
			switch (info.mJointConstraints[i].mType)
			{
			case JSCT_Equal:
				mMotionScript->SetPhase(info.mJointConstraints[i].mJointName1, params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			case JSCT_Negate:
				mMotionScript->SetPhase(info.mJointConstraints[i].mJointName1, -params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			}
		}
		else if (info.mJointConstraints[i].mBoundType == JBT_Constant)
		{
			switch (info.mJointConstraints[i].mType)
			{
			case JSCT_Equal:
				mMotionScript->SetConstant(info.mJointConstraints[i].mJointName1, params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			case JSCT_Negate:
				mMotionScript->SetConstant(info.mJointConstraints[i].mJointName1, -params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			}
		}
		else if (info.mJointConstraints[i].mBoundType == JBT_StraightLen1)
		{
			switch (info.mJointConstraints[i].mType)
			{
			case JSCT_Equal:
				mMotionScript->SetStaright1(info.mJointConstraints[i].mJointName1, params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			case JSCT_Negate:
				mMotionScript->SetStaright1(info.mJointConstraints[i].mJointName1, -params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			}
		}
		else if (info.mJointConstraints[i].mBoundType == JBT_StraightLen2)
		{
			switch (info.mJointConstraints[i].mType)
			{
			case JSCT_Equal:
				mMotionScript->SetStaright2(info.mJointConstraints[i].mJointName1, params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			case JSCT_Negate:
				mMotionScript->SetStaright2(info.mJointConstraints[i].mJointName1, -params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			}
		}
		else if (info.mJointConstraints[i].mBoundType == JBT_HalfRatio)
		{
			switch (info.mJointConstraints[i].mType)
			{
			case JSCT_Equal:
				mMotionScript->SetHalfRatio(info.mJointConstraints[i].mJointName1, params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			case JSCT_Negate:
				mMotionScript->SetHalfRatio(info.mJointConstraints[i].mJointName1, -params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			}
		}
		else if (info.mJointConstraints[i].mBoundType == JBT_Min)
		{
			switch (info.mJointConstraints[i].mType)
			{
			case JSCT_Equal:
				mMotionScript->SetRangeMin(info.mJointConstraints[i].mJointName1, params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			case JSCT_Negate:
				mMotionScript->SetRangeMin(info.mJointConstraints[i].mJointName1, -params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			}
		}
		else if (info.mJointConstraints[i].mBoundType == JBT_Max)
		{
			switch (info.mJointConstraints[i].mType)
			{
			case JSCT_Equal:
				mMotionScript->SetRangeMax(info.mJointConstraints[i].mJointName1, params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			case JSCT_Negate:
				mMotionScript->SetRangeMax(info.mJointConstraints[i].mJointName1, -params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			}
		}
		else if (info.mJointConstraints[i].mBoundType == JBT_Warp0)
		{
			switch (info.mJointConstraints[i].mType)
			{
			case JSCT_Equal:
				mMotionScript->SetSineWarpControlPts(info.mJointConstraints[i].mJointName1, 0, params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			case JSCT_Negate:
				mMotionScript->SetSineWarpControlPts(info.mJointConstraints[i].mJointName1, 0, -params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			}

		}
		else if (info.mJointConstraints[i].mBoundType == JBT_Warp1)
		{
			switch (info.mJointConstraints[i].mType)
			{
			case JSCT_Equal:
				mMotionScript->SetSineWarpControlPts(info.mJointConstraints[i].mJointName1, 1, params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			case JSCT_Negate:
				mMotionScript->SetSineWarpControlPts(info.mJointConstraints[i].mJointName1, 1, -params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			}

		}
		else if (info.mJointConstraints[i].mBoundType == JBT_Warp2)
		{
			switch (info.mJointConstraints[i].mType)
			{
			case JSCT_Equal:
				mMotionScript->SetSineWarpControlPts(info.mJointConstraints[i].mJointName1, 2, params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			case JSCT_Negate:
				mMotionScript->SetSineWarpControlPts(info.mJointConstraints[i].mJointName1, 2, -params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			}

		}
		else if (info.mJointConstraints[i].mBoundType == JBT_Warp3)
		{
			switch (info.mJointConstraints[i].mType)
			{
			case JSCT_Equal:
				mMotionScript->SetSineWarpControlPts(info.mJointConstraints[i].mJointName1, 3, params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			case JSCT_Negate:
				mMotionScript->SetSineWarpControlPts(info.mJointConstraints[i].mJointName1, 3, -params[info.mJointConstraints[i].mConstraintToJointIdx]);
				break;
			}

		}

	}

}

DecoArchive& operator<< (DecoArchive& Ar, const DecoArticulatedObject& obj)
{
	Ar << obj.name;
	Ar << obj.mCurrentTime;
	Ar << obj.mLocalToWorld;
	Ar << obj.mSimulateType;
	Vecd q = obj.mModel->getQ();
	Vecd qdot = obj.mModel->getQDot();
	int numDofs = obj.mModel->getNumDofs();
	Ar << numDofs;
	for (int i = 0; i < numDofs; ++i)
	{
		Ar << q[i];
		Ar << qdot[i];
	}
	const DecoSolidObject* solidObj = static_cast<const DecoSolidObject*>(&obj);
	Ar << (*solidObj);
	Ar << (*obj.mSimulator);
	return Ar;
}
DecoArchive& operator>> (DecoArchive& Ar, DecoArticulatedObject& obj)
{
	Ar >> obj.name;
	obj.ReadFromSkeleton(obj.name);
	Ar >> obj.mCurrentTime;
	Ar >> obj.mLocalToWorld;
	int simType;
	Ar >> simType;
	obj.mSimulateType = (ArticulatedSimType)simType;
	int numDofs = 0;
	Ar >> numDofs;
	int checkNumDofs = obj.mModel->getNumDofs();
	assert(numDofs == checkNumDofs);
	double q = 0;
	double qdot = 0;

	for (int i = 0; i < numDofs; ++i)
	{
		Ar >> q;
		Ar >> qdot;
		obj.mModel->getDof(i)->setValue(q);
		obj.mModel->getDof(i)->setVelocity(qdot);
	}
	DecoSolidObject* solidObj = static_cast<DecoSolidObject*>(&obj);
	Ar >> (*solidObj);

	obj.mSimulator = new ForwardDynamics();
	obj.mSimulator->SetModel(obj.mModel);

	Ar >> (*(obj.mSimulator));
	obj.mGeneralizedForce = new FluidSparseVector[numDofs];

	return Ar;
}
